/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Nikolaus Vahrenkamp
* @copyright  2013 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/
#ifndef __MMM_Math_Tools_H_
#define __MMM_Math_Tools_H_

#include "MMMCore.h"
#include <Eigen/Core>
#include <string> 
#include <vector>
#include <map>

#ifdef ALGLIB_AVAILABLE
#include <interpolation.h>
#endif

#include "MMMImportExport.h"

namespace MMM
{
    /*!
        \brief Several convenient math methods
    */
	namespace Math
	{
		//! Convert roll pitch yaw to homogeneous matrix with zero translation part
		Eigen::Matrix4f rpy2eigen4f (float r, float p, float y);

		//! Convert Quaternion to homogeneous matrix with zero translation part
		Eigen::Matrix4f quat2eigen4f( float x, float y, float z, float w );

		//! Converts rotation Matrix3f to intrinsic Euler angles (X Z' Y'')
		Eigen::Vector3f matrix3fToEulerXZY( const Eigen::Matrix3f &m);

		//! Converts rotational part of homogeneous Matrix4f to intrinsic Euler angles (X Z' Y'')
		Eigen::Vector3f matrix4fToEulerXZY( const Eigen::Matrix4f &m);

		//! Converts homogeneous Matrix4f to 6d vector consisting of position and intrinsic Euler angles (X Z' Y'') 
		Eigen::VectorXf matrix4fToPoseEulerXZY(const Eigen::Matrix4f &m);

		//! Converts homogeneous Matrix4f to 6d vector consisting of position and roll pitch yaw angles 
		Eigen::VectorXf matrix4fToPoseRPY(const Eigen::Matrix4f &m);

		//! Converts intrinsic Euler angles (X Z'' Y'') to rotation Matrix3f
		Eigen::Matrix3f eulerXZYToMatrix3f( const Eigen::Vector3f &eulerXZY );

		//! Converts pose vector consisting of position and intrinsic Euler angles (X Z' Y'') to homogeneous Matrix4f
		Eigen::Matrix4f poseEulerXZYToMatrix4f( const Eigen::Vector3f &pos, const Eigen::Vector3f &eulerXZY );

		//! Converts pose vector consisting of position and RPY angles to homogeneous Matrix4f
		Eigen::Matrix4f poseRPYToMatrix4f( const Eigen::Vector3f &pos, const Eigen::Vector3f &rpy );

        //! Linear Interpolation of Vector3f
        Eigen::Vector3f linearInterpolation(const Eigen::Vector3f &value1, float v1_timestep, const Eigen::Vector3f &value2, float v2_timestep, float timestep);

        //! Linear Interpolation of float values
        float linearInterpolation(float value1, float v1_timestep, float value2, float v2_timestep, float timestep);

        //! Returns a rounded float
        float roundf(float f, uint8_t precision = 3);

        //! Checks two float if they are equal
        bool equal(float f1, float f2);

#ifdef ALGLIB_AVAILABLE
        class MMM_IMPORT_EXPORT SplineFunction
        {
        public:
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW

            SplineFunction(Eigen::VectorXf input);
            Eigen::VectorXf derivative;
            Eigen::VectorXf derivative2ndOrder;
        };
#endif
	}
}

#endif 
